#ifndef CatRobot_h
#define CatRobot_h

#include <Servo.h>
#include <SoftwareSerial.h>

#define SERVONUM 12  // 伺服电机数量

class CatRobot {
public:
  // 构造函数
  CatRobot();
  
  // 初始化函数（需在setup中调用）
  void initServos();
  
  // 核心运动学计算函数
  void lengthangles();
  float gammaleft(float dy, float dz);
  float gammaright(float dy, float dz);
  float legleft(float dx, float dz, float gamma);
  float legright(float dx, float dz, float gamma);
  float beta(float leg);
  float alfafront(float dx, float beta, float leg);
  float alfarear(float dx, float beta, float leg);
  void turnpos(float f);
  
  // 伺服控制函数
  void servopos();
  void servomove();
  void servomovefast();
  
  // 脚部/身体运动函数
  void footxyz(int i, float x, float y, float z);
  void footmovexyz(int i, float x, float y, float z);
  void bodyxyz(float x, float y, float z);
  void bodymovexyz(float x, float y, float z);
  void bodytwist(float f);
  void footstep(int i, float x, float y);
  void footstepangle(int i, float f);
  
  // 步态函数
  void forwardcreep();
  void reversecreep();
  void leftturn();
  void rightturn();
  
  // 动作函数
  void sit();
  void stand();
  void handShake();
  void dance1();
  void dance2();
  void dance3();
  void kick();

private:
  Servo servo[SERVONUM];  // 伺服对象
  float servodeg0[SERVONUM];
  float servodegnew[SERVONUM];
  float servodegold[SERVONUM];
  int servodir[SERVONUM];
  float jointangle0[SERVONUM];
  float jointangle[SERVONUM];
  float footpos[12];
  float stepturn[12];
  float leglength[4];
  
  const float pi = 3.1416;
  const float alfa0 = pi / 6;
  const float beta0 = pi / 3;
  const float jointlength = 50;
  const float width = 120;
  const float leng = 120;
  const float distag = 12;
  const float toeout0 = 20;
  const float leglength0 = 2 * jointlength * cos(alfa0);
  const float gamma0 = asin(toeout0 / (leglength0 + distag));
  const float bodyradius = sqrt(pow((width / 2), 2) + pow((leng / 2), 2));
  const float phi0 = atan(width / leng);
  const float height0 = sqrt(pow(leglength0 + distag, 2) - pow(toeout0, 2));
  
  unsigned long timestep;
  int steplength;
  float phi;
};

#endif